#include "config.h"
#include "os.h"
#include "kernel.h"
#include "timer.h"
#include "event.h"
#include "util.h"
#include "queue.h"
#include "led.h"
#include "malloc.h"
#include <avr/io.h>
#include <string.h>

/**
 * \file kernel.c
 * \author Clecio Varjao and Leanne Ross
 * \date October 2007
 * \remarks Based on code created by Dr. Mantis Cheng
 * \brief RTOS kernel implementation
 */

volatile static QUEUE *Qp;	//!< Queue for Periodic tasks
volatile static QUEUE *Qs;	//!< Queue for System tasks
volatile static QUEUE *Qr;	//!< Queue for RR tasks


volatile TASK_EXECUTER		*curTE;	//!< Process_Executer of the currently RUNNING task.


volatile static TASK_CONSTRUCTOR	*idleTC;	//!< Task_Constructor for the IDLE task
volatile static TASK_DESCRIPTOR		*idleTD;	//!< Task_Descriptor for the IDLE task
volatile static TASK_EXECUTER		*idleTE;	//!< Task_Executer for the IDLE task


/*!
 *\brief Pointer for the Kernel Stack Pointer
 * Since this is a "full-served" model, the kernel is executing using its own
 * stack. We can allocate a new workspace for this kernel stack, or we can
 * use the stack of the "main()" function, i.e., the initial C runtime stack.
 * (Note: This and the following stack pointers are used primarily by the
 *   context switching code, i.e., Enter_Kernel(), which is written in assembly
 *   language.)
 */         
volatile unsigned char *KernelSp;

/*!
 *\brief Pointer for the current running task
 * This is a "shadow" copy of the stack pointer of "Cp", the currently
 * running task. During context switching, we need to save and restore
 * it into the appropriate process descriptor.
 */
volatile unsigned char *CurrentSp;

volatile unsigned int abortReason=0;	//!< Abort reason code


/** 1 if kernel has been started; 0 otherwise. */
volatile unsigned char KernelActive;  //!< Indicate whether the kernel is active or not


volatile static unsigned char timerStarted; //!< Indicate whether the timer is running or not


/*!
 *\brief set a task to be executed
 *\param e the task to be executed
 *
 * Change \c curTE to \a e, which will be executed when exit the kernel
*/
static void activate(TASK_EXECUTER *e){	
	curTE=e;
	curTE->ticks=0;
	curTE->task->state=RUNNING;
	CurrentSp = curTE->task->sp;
	if (timerStarted==0 && (curTE->level==PERIODIC || curTE->level==RR) ){
		kernel_start_timer();
		timerStarted=1;
	}
}

/*!
 *\brief Triggers an error
 *\param errorCode The error code to be triggered
 *
 *This function triggers the \c errorCode, and will call \c OS_Abort()
*/
void OS_Error(int errorCode){
	abortReason=errorCode;
	OS_Abort();
}

/*!
 *\brief Internal kernel function to cause a task to wait
 *\param e task that will be wait
 *
 * Changes the task state to WAITING, and if it is a RR task remove it from the RR queue.
*/
inline void kernel_wait_task(TASK_EXECUTER *e){
	e->task->state = WAITING;
	if (e->level==RR){ // Remove from the Queue
		queue_remove((QUEUE*)Qr, e);
	}
}

/*!
 *\brief Internal kernel function to signal a task
 *\param e task that will be signalized
 *
 * Changes the task state to READY, and enqueue the task to the correponding queue
*/
inline void kernel_signal_task(TASK_EXECUTER *e){
	e->task->state = READY;
	if (e->level==SYSTEM){
		enqueue((QUEUE*)Qs, e);
	}else{
		enqueue((QUEUE*)Qr, e);
	}
}

/**
  *\brief Kernel function that selects and controls the next task to be executed
  *
  * This internal kernel function is a part of the "scheduler". It chooses the 
  * next task to run.
  */
static void Dispatch() {
	
	volatile TASK_EXECUTER *n;
	volatile TASK_EXECUTER *cur; // curPE
	int nextLevel=-1;

	n=NULL;	
	if (curTE->level==SYSTEM && curTE->task->state==RUNNING){ //running SYSTEM task
		if (curTE->owner!=NULL){ // if it interrupted a PPP task
			if (curTE->owner->ticks > curTE->owner->maxTicks){ // exceed the time of the PPP task
				OS_Error(201); //PPP Time out
			}
		}
	} else {
		cur=(TASK_EXECUTER*)curTE;
		n=(TASK_EXECUTER*)dequeue((QUEUE*)Qs);

		if (n==NULL){			
			if(curTE->request==TICKTACK){
				if ( curTE->owner!=NULL){ // if it is a SYSTEM or ROUND ROBIN task
					if (curTE->owner->ticks >= curTE->owner->maxTicks) {
						nextLevel=PERIODIC;
					}else if ( curTE->ticks ==MAX_RR_TICKS) {
						nextLevel=RR;
					}					
				}else if ( curTE->ticks == curTE->maxTicks ){ // Switch Task (if possible?)
					 if (curTE->level==RR){
					 	nextLevel=RR;
					}
				}else if ( curTE->ticks > curTE->maxTicks ){ //Time out
					if (curTE->level==PERIODIC){
						OS_Error(202); //PPP Time out
					}
				}
			}

			if(curTE->request==NEXT){
				if ( curTE->owner!=NULL ){ // if there is an owner, which is a PPP task
					// if there is Time to Execute next RR
					if (curTE->owner->ticks < curTE->owner->maxTicks) {
						nextLevel=RR;
					}else{
						nextLevel=PERIODIC;
					}
				}else{
					if ( curTE->level!=PERIODIC ){
						if (Qp->next!=NULL){
							nextLevel=PERIODIC;
						} else {
							nextLevel=RR;
						}
					}else if (curTE->level==PERIODIC){
						if (curTE->ticks == curTE->maxTicks) {
							nextLevel=PERIODIC;
						}else if (curTE->ticks > curTE->maxTicks) { // Shouuld be captured by the TickTack (?)
							OS_Error(203); //PPP Time out
						}else{
							nextLevel=RR;
						}
					}
				}
			}
		}

		if (nextLevel>=0){ //Change task
			if(nextLevel==PERIODIC) {
				n=(TASK_EXECUTER*)dequeue((QUEUE*)Qp); 	// NEXT PPP
				if (n!=NULL){
					if (n->task->info->level==IDLE){
						//release current task
						curTE=n;
						nextLevel=RR;
					}
				} else {
					nextLevel=RR;
				}
			}

			if(nextLevel==RR) {
				n=(TASK_EXECUTER*)dequeue((QUEUE*)Qr);
				if (n==NULL){
					n=(TASK_EXECUTER*)idleTE;
					n->level=RR;
				}
				if(n!=curTE){
					n->owner=NULL;					
				}				
				if (curTE->level==PERIODIC){
					n->owner=(TASK_EXECUTER*)curTE;
				}else if (curTE->owner->level==PERIODIC){
					n->owner=curTE->owner;
				}
			}		
		}

		if(n!=NULL){
			//Activate Task
			activate((TASK_EXECUTER*)n);
		}

	}

} //end-function


/**
 * \brief Kernel IDLE function
 * 
 * Executed by the IDLE Task
 */
void IDLE_TASK(){
	for(;;);	
}

/**
 *\brief Terminate a task
 *
 *Responsible for terminate the current task and release all resources allocated for it.
 */
void Kernel_Terminate_Cur_Task(){
	TASK_EXECUTER *cur;
	cur=(TASK_EXECUTER*)idleTE;
	if (curTE->level==SYSTEM || curTE->level==RR){
		if (curTE->level==RR){
			//Remove from the Queue
			queue_remove((QUEUE*)Qr, (void*)curTE);
		}

		if (curTE->owner!=NULL){ // owned by a Periodic task
			cur=curTE->owner;
		}
		//Release Task Constructor
		TC_free(curTE->task->info);
		curTE->task->info=NULL;

		//Release Task Descriptor
		TD_free(curTE->task);
		curTE->task=NULL;

		TE_free((TASK_EXECUTER*)curTE);

	}else{ // PERIODIC TASK
		//Release Task Constructor
		TC_free(curTE->task->info);
		curTE->task->info=NULL;

		//Release Task Descriptor
		TD_free(curTE->task);

		//Switch to IDLE task
		curTE->task=(TASK_DESCRIPTOR*)idleTD;
	}

	activate(cur);
}

/**
 *\brief Main kernel loop, initially called by OS_Init().
 *
 * This internal kernel function is the "main" driving loop of this full-served
 * model architecture.
 */
void Next_Kernel_Request() {
	TASK_EXECUTER *aux;
	Dispatch();  /* select a new task to run */

   while(1) {
		curTE->request = NONE; /* clear its request */

		/* activate this newly selected task */
		CurrentSp = curTE->task->sp;
		Exit_Kernel();    /* or CSwitch() */

		/* if this task makes a system call, it will return to here! */

		/* save the curTask's stack pointer */
		curTE->task->sp = (unsigned char *)CurrentSp;
		
		switch(curTE->request){
		case CREATE:
			aux=Kernel_Create_And_Schedule(curTE->requestInfo);
			curTE->requestInfo=NULL;
			break;
		case NEXT:
			if (curTE->task->state != WAITING){
				curTE->task->state=READY;
			}
			Dispatch();
			break;
		case TICKTACK:
			curTE->ticks++;
			if (curTE->owner!=NULL){
				curTE->owner->ticks++;
			}

			Dispatch();	
			break;
		case TERMINATE:
			/* deallocate all resources used by this task */
			Kernel_Terminate_Cur_Task();
			curTE->request=NEXT;
			Dispatch();
			break;
		default:
		  /* Houston! we have a problem here! */
		  break;
		} //end-switch

    } //end-while
} //end-function

/**
 *Create a new TASK_EXECUTER
 */
TASK_EXECUTER *Kernel_Create_Executer(TASK_DESCRIPTOR *d){
	TASK_EXECUTER *e;

	e=TE_alloc();
	e->ticks=0;
	e->maxTicks=0;
	e->task=d;
	e->level=d->info->level;
	return (TASK_EXECUTER*)e;
}

/* function documented in os.h */
void OS_Init() 
{
	int i;
	TASK_EXECUTER	 *e;

	/*   PROCESSOR SPEED */
    CLKPR = (1<<CLKPCE); 	// Enable clock prescaler bit  1000 0000
    CLKPR = 0x00;			// Commit the change

	KernelActive = 0;
	timerStarted=0;
	Qp=queue_create(1); //circular
	Qs=queue_create(0); //non-circular
	Qr=queue_create(1); //circular

	kernel_init_timer();

	idleTC=TC_alloc();
	idleTC->level=IDLE;
	idleTC->arg=-1;
	idleTC->code=IDLE_TASK;
	idleTC->name='I';
	
	idleTD=Kernel_Create_Task((TASK_CONSTRUCTOR*)idleTC);
	idleTE=Kernel_Create_Executer((TASK_DESCRIPTOR*)idleTD);
	idleTE->name='I';

	//Initialize PPP Array. Initialize like all PPP tasks being idle
	for (i = 0; i < (PT*2); i+=2) {
		e=Kernel_Create_Executer((TASK_DESCRIPTOR*)idleTD);
		e->name=PPP[i];
		e->level=PERIODIC;
		e->maxTicks=PPP[i+1];
		enqueue((QUEUE*)Qp, e);
	}

	// Create main() task
	Task_Create((void (*)(void))main, 0, SYSTEM, 'M');

	//Start with the Idle Task requesting the next task
	idleTE->request=NEXT;
	
	activate((TASK_EXECUTER*)idleTE);

	KernelActive = 1;
	Next_Kernel_Request();

}//end-function

/* function documented in os.h */
void OS_Abort(void){
	Disable_Interrupt();
	init_leds();
	for(;;){
		blink_led(LED_D2|LED_D5);
		blink_led(~(LED_D2|LED_D5));
	}
} //end-function


/**
  *\brief Kernel interrupt handler function.
  *
  * Responsible to execute a TICKTACK request everytime that a time interrupt occurs.
  */
inline void kernel_tick(void){
	curTE -> request = TICKTACK;
	Enter_Kernel();
} //end-function


/**
  *  Schedule a TASK_DESCRIPTOR by putting into the right queue
  */
TASK_EXECUTER *Kernel_Schedule(TASK_DESCRIPTOR *d) {
	TASK_EXECUTER *e;
	TASK_EXECUTER *aux;
	TASK_CONSTRUCTOR *c;
	QUEUE_ITEM *qi;


	e=NULL;
	c=d->info;
	if (c->level==PERIODIC){
		qi=Qp->first;
		if (qi!=NULL){ //Not empty
			do{
				aux=(TASK_EXECUTER*)qi->item;
				if (aux->name==d->info->name){
					e=aux;
					e->task=d;
					break;
				}
				qi=qi->next;
			} while (qi!=Qp->first);

			if (e==NULL){
				OS_Error(213);
			}
		}
	}else{
		e=Kernel_Create_Executer(d);
		if (c->level==SYSTEM){
			enqueue((QUEUE*)Qs, e);
		}else{	//RR task
			enqueue((QUEUE*)Qr, e);
		}
	}
	return e;
}

/**
 *\brief Create and schedule a task based on a Task Constructor
 *
 *A TASK_DESCRIPTOR is created and then scheduled for the corresponding queue
 */
TASK_EXECUTER *Kernel_Create_And_Schedule(TASK_CONSTRUCTOR *c) 
{

	TASK_DESCRIPTOR *d;
	TASK_EXECUTER *e;

	d=Kernel_Create_Task(c);
	e=Kernel_Schedule(d);

	return e;
} //end-function

/**
  *  Create a new TASK_DESCRIPTOR based on a Constructor
  */
TASK_DESCRIPTOR *Kernel_Create_Task(TASK_CONSTRUCTOR *c) {
	TASK_DESCRIPTOR *d;
	
	d=TD_alloc();
	Kernel_Create_Task_At(d, c);

	return d;
}


/**
 *\brief Initialize a Task Descriptor
 *
 * When creating a new task, it is important to initialize its stack just like
 * it has called "Enter_Kernel()"; so that when we switch to it later, we
 * can just restore its execution context on its stack.
 * (See file "cswitch.S" for details.)
 * \remarks based on code created by  Dr. Mantis Cheng
 */
void Kernel_Create_Task_At( TASK_DESCRIPTOR *d, TASK_CONSTRUCTOR *c) 
{   
   unsigned char *sp;

   sp = (unsigned char *) &(d->workSpace[WORKSPACE-1]);

   //Initialize the workspace (i.e., stack) and PD here!
   //Clear the contents of the workspace
   memset(&(d->workSpace),0,WORKSPACE);

   //Notice that we are placing the address (16-bit) of the functions
   //onto the stack in reverse byte order (least significant first, followed
   //by most significant).  This is because the "return" assembly instructions 
   //(rtn and rti) pop addresses off in BIG ENDIAN (most sig. first, least sig. 
   //second), even though the AT90 is LITTLE ENDIAN machine.

   //Store terminate at the bottom of stack to protect against stack underrun.
   *(unsigned char *)sp-- = ((unsigned int)Task_Terminate) & 0xff;
   *(unsigned char *)sp-- = (((unsigned int)Task_Terminate) >> 8) & 0xff;

   //Place return address of function at bottom of stack
   *(unsigned char *)sp-- = ((unsigned int)c->code) & 0xff;
   *(unsigned char *)sp-- = (((unsigned int)c->code) >> 8) & 0xff;


#ifdef KERNEL_DEBUG
	int counter = 0;
   //Fill stack with initial values for development debugging
   //Registers 0 -> 31 and the status register
   for (counter = 0; counter < 33; counter++)
   {
      *(unsigned char *)sp-- = counter;
   }
#else
   //Place stack pointer at top of stack
   sp = sp - 33;
#endif
      
	d->sp = sp;			// stack pointer into the "workSpace" //
	d->info = c;		// function to be executed as a task //
	d->state = READY;
} //end-function
